﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Integrador.Controles
{
    class PID_Z : PID
    {
        public PID_Z() : base() { }
        /// <summary>
        /// Control PID
        /// </summary>
        /// <param name="x">estado actual</param>
        /// <param name="xd">estado deseado</param>
        /// <param name="xd">estado de los rotores anterior</param>
        
        public override double[] Control(Estado x, Estado xd, ref double[] RotAnt)
        {
            double errorP = 0, errorD = 0;
            //calculamos el error proporcional(por ahora solo nos interesa la altura)
            errorP = xd.Z - x.Z;

            //error integrativo

            errorI = errorI + (errorP * this.Tp);

            //error derivativo

            errorD = (errorP - this.errorAnterior) / this.Tp;

            double P = errorP * Kp;
            double D = errorD * Kd;
            double I = errorI * Ki;
            double incremento = P + D + I;

            this.errorAnterior = errorP;
            if (incremento >= 3)
            {
                incremento = 3;
            }
            if (incremento <= 0)
            {
                incremento = 0;
            }
            //como es la altura incrementamos todos los rotores
            for (int i = 0; i < RotAnt.Length; i++)
            {
                RotAnt[i] = incremento;
            }

            return new double[] { errorP, errorD, errorI, errorD + errorP + errorI };

        }
    }
}
